# 四足机器人姿态计算模块，用于计算各腿的位置
#Copyright Deng（灯哥） (ream_d@yeah.net)  Py-apple dog project
#Github:https://github.com/ToanTech/py-apple-quadruped-robot
#Licensed under the Apache License, Version 2.0 (the "License");
#you may not use this file except in compliance with the License.
#You may obtain a copy of the License at:http://www.apache.org/licenses/LICENSE-2.0

from math import sin,cos,pi
import time


def cal_ges(PIT,ROL,YA,bodyL,bodyW,w,x,Hc):
    """
    计算四足机器人在给定姿态下各腿的位置。
    
    参数:
    PIT (float): 俯仰角（Pitch），单位：度
    ROL (float): 横滚角（Roll），单位：度
    YA (float): 偏航角（Yaw），单位：度
    bodyL (float): 机器人身体的长度
    bodyW (float): 机器人身体的宽度
    w (float): 机器人腿的宽度
    x (float): 机器人身体在x轴上的偏移量
    Hc (float): 机器人身体的高度
    
    返回:
    tuple: 包含各腿在x, y, z轴上的位置，顺序为腿1, 腿2, 腿3, 腿4
    """
    # 预先计算常用三角函数值
    P = PIT * pi / 180
    R = ROL * pi / 180
    Y = YA * pi / 180
    
    sinP = sin(P)
    cosP = cos(P)
    sinR = sin(R)
    cosR = cos(R)
    sinY = sin(Y)
    cosY = cos(Y)
    
    # 计算重复使用的组合项
    cosP_cosY = cosP * cosY
    cosP_sinY = cosP * sinY
    cosR_cosY = cosR * cosY
    sinP_sinR_sinY = sinP * sinR * sinY
    cosR_sinY = cosR * sinY
    cosY_sinP_sinR = cosY * sinP * sinR
    sinR_sinY = sinR * sinY
    cosR_cosY_sinP = cosR * cosY * sinP
    cosY_sinR = cosY * sinR
    cosR_sinP_sinY = cosR * sinP * sinY
    
    # 腿1的位置计算
    ABl_x = bodyL/2 - x - (bodyL * cosP_cosY)/2 + (bodyW * cosP_sinY)/2
    ABl_y = w/2 - (bodyW * (cosR_cosY + sinP_sinR_sinY))/2 - (bodyL * (cosR_sinY - cosY_sinP_sinR))/2
    ABl_z = -Hc - (bodyW * (cosY_sinR - cosR_sinP_sinY))/2 - (bodyL * (sinR_sinY + cosR_cosY_sinP))/2
    
    # 腿2的位置计算
    AB2_x = bodyL/2 - x - (bodyL * cosP_cosY)/2 - (bodyW * cosP_sinY)/2
    AB2_y = (bodyW * (cosR_cosY + sinP_sinR_sinY))/2 - w/2 - (bodyL * (cosR_sinY - cosY_sinP_sinR))/2
    AB2_z = (bodyW * (cosY_sinR - cosR_sinP_sinY))/2 - Hc - (bodyL * (sinR_sinY + cosR_cosY_sinP))/2
    
    # 腿3的位置计算
    AB3_x = (bodyL * cosP_cosY)/2 - x - bodyL/2 + (bodyW * cosP_sinY)/2
    AB3_y = w/2 - (bodyW * (cosR_cosY + sinP_sinR_sinY))/2 + (bodyL * (cosR_sinY - cosY_sinP_sinR))/2
    AB3_z = (bodyL * (sinR_sinY + cosR_cosY_sinP))/2 - (bodyW * (cosY_sinR - cosR_sinP_sinY))/2 - Hc
    
    # 腿4的位置计算
    AB4_x = (bodyL * cosP_cosY)/2 - x - bodyL/2 - (bodyW * cosP_sinY)/2
    AB4_y = (bodyW * (cosR_cosY + sinP_sinR_sinY))/2 - w/2 + (bodyL * (cosR_sinY - cosY_sinP_sinR))/2
    AB4_z = (bodyW * (cosY_sinR - cosR_sinP_sinY))/2 - Hc + (bodyL * (sinR_sinY + cosR_cosY_sinP))/2
    
    # 将计算结果重新排列为x, y, z坐标
    x1=ABl_x
    y1=ABl_z
    z1=ABl_y

    x2=AB2_x
    y2=AB2_z
    z2=AB2_y

    x3=AB4_x
    y3=AB4_z
    z3=AB4_y

    x4=AB3_x
    y4=AB3_z
    z4=AB3_y
    
    return x1,x2,x3,x4,y1,y2,y3,y4,z1,-z2,-z3,z4

'''
def cal_ges(pit,rol,ya,l,b,w,x,Hc):
    rol=-rol
    Hc0=Hc;Hc1=Hc;Hc2=Hc;Hc3=Hc
    PI=pi
    #弧度-角度转换
    if float(pit) >30:
        pit=30
    elif float(pit)<-30:
        pit=-30
    if float(rol) > 50:
        rol=50
    elif float(rol)<-40:
        rol=-50
    if float(ya) > 15:
        ya=15
    elif float(ya)<-15:
        ya=-15

    a=float(pit)/180.0*PI
    d=float(rol)/180.0*PI
    z_yaw=float(ya)
    #弧度-角度转换
    c=Hc-(l/2)*sin(d)
    b=Hc-(b/2)*sin(a)
    Hc2=c+b-Hc     #腿3高度
    Hc1=2*Hc-Hc2     #腿2高度
    Hc0=2*c-Hc2    #腿1高度
    Hc3=2*Hc-Hc0    #腿4高度
    d=cos(d)*sin(d)
    E=z_yaw/180.0*PI    #E为航向角

    x0=-Hc0*cos(a)*sin(a)+(150*cos(0.2*PI-E)-121)    #a为俯仰角
    y0=-Hc0*cos(a)*cos(a)/cos(d)    #d为横滚角
    z0=Hc0*d+(150*sin(0.2*PI-E)-40)

    x1=-Hc1*cos(a)*sin(a)-(121-150*cos(0.2*PI+E))    #a为俯仰角
    y1=-Hc1*cos(a)*cos(a)/cos(d)    #d为横滚角
    z1=-Hc1*d+150*sin(0.2*PI+E)-40

    x2=Hc2*cos(a)*sin(a)-(121-150*cos(0.2*PI+E))
    y2=-Hc2*cos(a)*cos(a)/cos(d)
    z2=Hc2*d+150*sin(0.2*PI+E)-40

    x3=Hc3*cos(a)*sin(a)+(150*cos(0.2*PI-E)-121)
    y3=-Hc3*cos(a)*cos(a)/cos(d)
    z3=-Hc3*d+(150*sin(0.2*PI-E)-40)
    
    return x0-x,x1-x,x3-x,x2-x,y0,y1,y3,y2,z0,z1,z3,z2


'''
